
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Sensor.h
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "ErrorMode.h"
#include "Vibration.h"
#include <common/gp_math/gp_mathlib.h>
#include <common/location/location.h>
#include <stdlib.h>

/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    ErrorMode  err_mode;
    VibrationMode vibe;
    Vector3f_t sensor_data;
}Imu;

typedef struct
{
    ErrorMode  err_mode;
    Vector3f_t mag_data;
}Magnetometer;

typedef struct
{
    //数据
    float tempc;
    float pressure_pa;

    //干扰
    float bias;
    float variance;
}Barometer;

typedef struct
{
    int32_t lat;    //经纬高
    int32_t lon;
    int32_t alt;

    uint16_t eph;    //水平和纵向精度因子
    uint16_t epv;
    uint16_t vel;    //地速

    int16_t vn;        //惯性系下的速度
    int16_t ve;
    int16_t vd;

    uint16_t cog;
    uint8_t fix_type;
    uint8_t statellites_visible;
}GPS_Data;

typedef struct
{
    GPS_Data gps_data;    //gps数据
    //位置和速度方差
    Vector3f_t pos_variance;
    Vector3f_t vel_variance;
    int32_t lat_meas, lon_meas, h_meas;
    Vector3f_t vel_meas;
}GPS;

typedef struct
{
    Imu             Gyro;            //陀螺仪
    Imu             Accel;            //加速度计
    Magnetometer Mag;    //磁力计
    Barometer     Baro;
    GPS             Gps;
}Sensor;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void ImuMode_Run(Imu* pImu, Vector3f_t* state_in, float* prop_vel, float dt);
void MagMode_Run(Magnetometer* pMag, Location* loc, Quat_t* quat, float dt);
void Brometer_Run(Barometer* pBro, float height, float T);
void GPSMode_Run(GPS* pGPS, Location* loc, Location* velocity_ef, float dt);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif

